#include <Wire.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define REAL_TIME_DISTANCE 0x00//实时距离寄存器

//Mega2560 20(SDA),21(SCL)
uint8_t RX_BUF[2]= {0};//接收数据缓冲区
uint16_t distance = 0;//距离
uint16_t high_byte = 0;
uint16_t low_byte = 0;

int data = 0;
int cnt = 0;

void setup() {
  // put your setup code here, to run once:
Wire.begin();//加入IIC总线，设置为主机
Serial.begin(9600);//启动串口调试
}


void loop() {
Serial.println("I'm here");
  
if(Serial.available()>0){
  
    data = Serial.read();
    
    if(data == 'H'){
      
 for(uint8_t i=0;i<5;i++){
    SensorRead(REAL_TIME_DISTANCE,RX_BUF,2);
    high_byte += RX_BUF[0];
    low_byte += RX_BUF[1];
  }
  high_byte /= 5;
  low_byte /= 5;
  distance = high_byte;
  distance <<= 8;
  distance += low_byte;
  high_byte = 0;
  low_byte = 0;
  Serial.println(distance);

delay(500);
    }
  }

delay(400);

}





//往IIC从机写入数据
//addr(参数寄存器地址),datbuf（数据缓冲区地址),cnt(数据长度)
String SensorWrite(uint8_t addr,uint8_t*datbuf,uint16_t cnt){
  uint8_t result = 0 ;
  Wire.beginTransmission(82);//指定传输从机地址，TOF1默认地址为#82
  result = Wire.write(addr);
  if(result == 0) return "write error!";
  delay(1);//数据手册建议t>=32us
  result = Wire.write(datbuf,cnt);
  if(result == 0) return "write error!";
  Wire.endTransmission();
}

//读取IIC从机数据
void SensorRead(uint8_t addr,uint8_t* datbuf,uint16_t cnt){
  Wire.beginTransmission(82);
  Wire.write(addr);
  Wire.endTransmission();
  delay(1);
  Wire.requestFrom(82,cnt);//向指定从机请求数据
  for(uint8_t i=0;i<cnt;i++){
    *datbuf = Wire.read();
    datbuf++;
  }
}
